% EKF based localization of a robot with known landmarks only
%% this should be a subset of your code for Q3

clear; clc; close all;
%load Q4_example_data.mat
load Q4_data.mat

T = length(u);
ss = length(x0);
xfilt = x0;
Sigma_filt{1} = Sigma0;

for t=1:T-1

% your code here [This will be a subset of your code for Q3]
    
    xfilt(:,t+1) = f_robot(xfilt(:,t), u(:,t), dt);
    A = jacobian_f_robot(xfilt(:,t), u(:,t), dt);
	Sigma_filt{t+1} = A*Sigma_filt{t}*A' + Q*dt;% your code here
    % dynamics update code here

    % for all landmarks, check if it was observed and if so then
    % incorporate the measurement.
    for l=1:landmarks.num_landmarks
        if(y_landmarks_valid{l}(t+1))
            C = jacobian_f_known_landmark(xfilt(:,t+1), landmarks.landmarks{l}.pos); % your code into jacobian_f_known_landmark.m
            ypred = f_known_landmark(xfilt(:,t+1), landmarks.landmarks{l}.pos); % your code into f_known_landmark.m
            y = y_landmarks{l}(:,t+1) ;% the measurement
            
            % some code here
            K = Sigma_filt{t+1}*C'*inv(C*Sigma_filt{t+1}*C' +  R_landmark);
            
            xfilt(:,t+1) =  xfilt(:,t+1) + K * (y - ypred); % code here
            Sigma_filt{t+1} = Sigma_filt{t+1} - K*C*Sigma_filt{t+1}; % code here
            
        end
    end
    %if(t==20)
     %   return
    %end
end



% plot subsampled trajectory:
colors1 = ['km']; 
map_fig_id = figure; hold on; axis([-5 25 -5 25]); axis equal; xlabel('East'); ylabel('North');
spacing = 20;
for i= [1:spacing:size(xfilt,2) size(xfilt,2)]
    plot_uncertainty_ellipse(xfilt(1:3,i), Sigma_filt{i}, map_fig_id, colors1);
end

